#include "motor.h"
#include "bsp_motor.h"
#include "bsp_pid.h"
#include "encoder.h"

#define PID_KP      (0.8f)
#define PID_KI      (0.06f)
#define PID_KD      (0.5f)

motor_pid_t pid_new;

/**
 * 电机控制初始化
 */
void motor_init(void)
{
	// PID 赋初值
	pid_new.target_val = 0.0;
	pid_new.pwm_output = 0.0;
	pid_new.err = 0.0;
	pid_new.err_last = 0.0;
	pid_new.err_next = 0.0;
	pid_new.integral = 0.0;
	pid_new.Kp = PID_KP;
	pid_new.Ki = PID_KI;
	pid_new.Kd = PID_KD;

	// PWM 赋初值
    PWM_M1_A = 0;
    PWM_M1_B = 0;
    PWM_M2_A = 0;
    PWM_M2_B = 0;
    PWM_M3_A = 0;
    PWM_M3_B = 0;
    PWM_M4_A = 0;
    PWM_M4_B = 0;
}

void motor_handler(void)
{
	/* 获取码盘和速度 */
	encoder_timer_cnt();

	/* */
	pid_new.target_val = 0.0f;
	float temp = encoder_dev.speed_M[0];

	PID_Incre_Calc(&pid_new, temp);

	int16_t pwm_out = pid_new.pwm_output;

	Motor_Set_Pwm(MOTOR_ID_M1, pwm_out);

	printf("pid_new out = %f, %f, %d.\r\n", pid_new.target_val, temp, pwm_out);
}

